#include "rclcpp/rclcpp.hpp"
#include "std_msgs/msg/string.hpp"
#include "controlcan.h"

using namespace std::chrono_literals;

// 3.自定义节点类；
class Mynode : public rclcpp::Node
{
public:
  Mynode() : Node("mynode_node_cpp")
  {
    // 初始化CAN设备
    device_type = VCI_USBCAN2; // 设备类型修改
    device_index = 0;          // 设备的索引
    can_index = 0;             // CAN控制器的索引 CAN1为0，CAN2为1

    if (VCI_OpenDevice(device_type, device_index, 0) != STATUS_OK)
    {
      RCLCPP_FATAL(this->get_logger(), "无法开启CAN设备");
      return;
    }

    if (VCI_ReadBoardInfo(device_type, device_index, &board_info) != STATUS_OK)
    {
      RCLCPP_FATAL(this->get_logger(), "获取设备信息失败");
      VCI_CloseDevice(device_type, device_index);
      return;
    }

    // 设置CAN初始化配置
    init_config.AccCode = 0X00000000;
    init_config.AccMask = 0xFFFFFFFF;
    init_config.Filter = 2; // 只接受标准帧
    init_config.Mode = 0;
    init_config.Timing0 = 0x00;
    init_config.Timing1 = 0x14; // 波特率：1000kps

    if (VCI_InitCAN(device_type, device_index, can_index, &init_config) != STATUS_OK)
    {
      RCLCPP_FATAL(this->get_logger(), "------初始化CAN参数失败------");
      VCI_CloseDevice(device_type, device_index);
      return;
    }

    // 开始CAN通讯
    if (VCI_StartCAN(device_type, device_index, can_index) != STATUS_OK)
    {
      RCLCPP_FATAL(this->get_logger(), "------开启CAN通道失败------");
      VCI_CloseDevice(device_type, device_index);
      return;
    }


    // BYTE MODLE[8] = {0X2F, 0X60, 0X60, 0X00, 0X03, 0X00, 0X00, 0X00};

    // BYTE Open1[8] = {0X2B, 0X40, 0X60, 0X00, 0X06, 0X00, 0X00, 0X00};
    // BYTE Open2[8] = {0X2B, 0X40, 0X60, 0X00, 0X0F, 0X00, 0X00, 0X00};
    // BYTE Open3[8] = {0X2B, 0X40, 0X60, 0X00, 0X03, 0X00, 0X00, 0X00};

    // BYTE Acc[8] = {0X23, 0X83, 0X60, 0X00, 0X0A, 0X00, 0X00, 0X00};
    // BYTE Dec[8] = {0X23, 0X84, 0X60, 0X00, 0X0A, 0X00, 0X00, 0X00};
    // BYTE Max[8] = {0X23, 0X81, 0X60, 0X00, 0XD0, 0X07, 0X00, 0X00};

    BYTE config_motor1[8] = {0x2F, 0x60, 0x60, 0x00, 0x03, 0x00, 0x00, 0x00}; //  速度模式
    BYTE config_motor2[8] = {0x2B, 0x40, 0x60, 0x00, 0x0F, 0x00, 0x00, 0x00}; //  启动+电压输出+允许急停+允许操作
    BYTE config_motor3[8] = {0x23, 0x83, 0x60, 0x00, 0xE8, 0x06, 0x00, 0x00}; //  设置电机加速度
    BYTE config_motor4[8] = {0x23, 0xF9, 0x60, 0x01, 0x0C, 0x17, 0x00, 0x00}; //  设置速度环比例系数
    BYTE config_motor5[8] = {0x2B, 0xF9, 0x60, 0x02, 0x1E, 0x00, 0x00, 0x00}; //  设置速度环积分时间
    // BYTE speed_data5[8] = {0x2B, 0x40, 0x60, 0x00, 0x00, 0x00, 0x00, 0x00};   // 设置速度为0,去使能

    SendData(config_node, 0x602, config_motor1);
    usleep(100000);
    SendData(config_node, 0x602, config_motor2);
    usleep(100000);
    SendData(config_node, 0x602, config_motor3);
    usleep(100000);
    SendData(config_node, 0x602, config_motor4);
    usleep(100000);
    SendData(config_node, 0x602, config_motor5);
    usleep(100000);

    BYTE Speed_data1[8] = {0X23, 0XFF, 0X60, 0X00, 0X00, 0X00, 0X00, 0X00};
    BYTE Speed_data2[8] = {0X23, 0XFF, 0X60, 0X00, 0XE8, 0X03, 0X00, 0X00};

    // // 发送数据
    // // SendData(config_node, 0x602, Open1);
    // SendData(config_node, 0x602, Open2);
    // // SendData(config_node, 0x602, Open3);

    // SendData(config_node, 0x602, MODLE);

    // SendData(config_node, 0x602, Acc);
    // SendData(config_node, 0x602, Dec);
    // SendData(config_node, 0x602, Max);
    SendData(send_motor_speed, 0x602, Speed_data1);
    usleep(100000);
    SendData(send_motor_speed, 0x602, Speed_data2);

    // RCLCPP_INFO(this->get_logger(), "------电机已启动------");
    // SendData(config_node, 0x602, Open1);
    // std::this_thread::sleep_for(100ms);
    // RCLCPP_INFO(this->get_logger(), "------数据已发送------");
    // // SendData(config_node, 0x602, torque_2);
    // // std::this_thread::sleep_for(5s);
    // RCLCPP_INFO(this->get_logger(), "------数据已发送------");
    // SendData(config_node, 0x602, torque_1);
    // std::this_thread::sleep_for(5s);
    // SendData(config_node, 0x602, close);
    // std::this_thread::sleep_for(100ms);
    // RCLCPP_INFO(this->get_logger(), "------电机已关闭------");
    // std::this_thread::sleep_for(110s);
    // VCI_CloseDevice(device_type, device_index);
    // RCLCPP_INFO(this->get_logger(), "------关闭CAN设备------");
  }

private:
  rclcpp::Publisher<std_msgs::msg::String>::SharedPtr publisher_;
  // USB-CAN系列接口卡的设备信息
  VCI_BOARD_INFO board_info;
  // 初始化CAN的配置
  VCI_INIT_CONFIG init_config;
  // CAN帧结构体
  VCI_CAN_OBJ config_node;
  VCI_CAN_OBJ send_motor_speed;


  DWORD device_type;
  DWORD device_index;
  DWORD can_index;

  void SendData(VCI_CAN_OBJ &handle_obj, const int id, const BYTE *data)
  {
    // 帧ID
    handle_obj.ID = id;
    // 是否是远程帧。=0时为数据帧，=1时为远程帧（数据段空）。
    handle_obj.RemoteFlag = 0;
    // 是否是扩展帧。=0时为标准帧（11位ID），=1时为扩展帧（29位ID）。
    handle_obj.ExternFlag = 0;
    // 数据长度 DLC (<=8)，即CAN帧Data有几个字节。
    handle_obj.DataLen = 8;
    for (int i = 0; i < 8; i++)
    {
      handle_obj.Data[i] = data[i];
    }
    // VCI_Transmit（发送函数） 设备类型 设备索引 CAN通道索引 要发送的帧结构体 VCI_CAN_OBJ数组的首指针 发送的帧数量
    if (VCI_Transmit(device_type, device_index, can_index, &handle_obj, 1) > 0)
    {
    }
    else
    {
      RCLCPP_FATAL(this->get_logger(), "------USB-CAN设备不存在或USB掉线------");
    }
  }
};

int main(int argc, char const *argv[])
{
  // 2.初始化ROS2客户端；
  rclcpp::init(argc, argv);
  // 4.调用spain函数，并传入节点对象指针；
  rclcpp::spin(std::make_shared<Mynode>());
  // 5.资源释放
  VCI_CloseDevice(VCI_USBCAN2, 0);
  rclcpp::shutdown();
  return 0;
}